package com.hitqz.robot.arm;

import cn.hutool.core.util.StrUtil;
import com.hitqz.robot.api.business.enums.BrandEnum;
import com.hitqz.robot.arm.dto.*;
import com.hitqz.robot.arm.util.RealManToolkit;
import com.hitqz.robot.camera.IEquipmentService;
import com.hitqz.robot.camera.configure.EquipmentCmdEnum;
import com.hitqz.robot.camera.dto.*;
import com.hitqz.robot.camera.exception.EquipmentException;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import java.util.List;

/**
 *
 * @author hongjiasen
 * @date 2024/12/27
 */
@Service("realManArm")
@Slf4j
public class RealManArmService implements IEquipmentService {

    private RealManToolkit toolkit;
    private String ip;

    @Override
    public BrandEnum brand() {
        return BrandEnum.REALMAN;
    }

    @Override
    public boolean intSdk() {
        return true;
    }

    @Override
    public boolean login(EquipmentLoginParamsDto dto) {
        ip = dto.getIp();
        toolkit = RealManToolkit.asClient(ip,dto.getPort());
        toolkit.setArmInt();
        return Boolean.TRUE;
    }

    @Override
    public IEquipmentCmdResult executeCmd(QueryParamsDto dto, boolean async, EquipmentCmdEnum cmdEnum, IEquipmentCmdParamsDto cmdParamsDto) {
        if (StrUtil.isBlank(ip)) {
            log.info("未初始化机械臂控制，执行初始化");
            EquipmentLoginParamsDto loginParamsDto = new EquipmentLoginParamsDto();
            loginParamsDto.setIp(dto.getIp());
            loginParamsDto.setPort(dto.getPort());
            try {
                this.login(loginParamsDto);
            } catch (Exception e) {
                throw new EquipmentException("机械臂连接失败，请检查机械臂配置与系统外设配置");
            }
        }
        if (cmdEnum == EquipmentCmdEnum.SET_ZERO_POINT) {
            RealManArmStateDto stateDto = toolkit.getCurrentStatus();
            List<Integer> list = stateDto.getJoint();
            if (list.size() > 6) {
                toolkit.setInitPose(list.get(0), list.get(1), list.get(2)
                        , list.get(3), list.get(4), list.get(5), list.get(6));
            } else {
                toolkit.setInitPose(list.get(0), list.get(1), list.get(2)
                        , list.get(3), list.get(4), list.get(5), null);
            }
        }
        if (cmdEnum == EquipmentCmdEnum.TURN_ZERO_POINT) {
            List<Integer> list = toolkit.getInitPose();
            if (list.size() > 6) {
                toolkit.moveJoints(list.get(0), list.get(1), list.get(2)
                        , list.get(3), list.get(4), list.get(5), list.get(6), 50);
            } else {
                toolkit.moveJoints(list.get(0), list.get(1), list.get(2)
                        , list.get(3), list.get(4), list.get(5), null, 50);
            }
        }
        if (cmdEnum == EquipmentCmdEnum.GET_ARM_POWER_STATE) {
            RealManArmPowerStateDto result = new RealManArmPowerStateDto();
            result.setPowerState(toolkit.getArmPowerState());
            return result;
        }
        if (cmdEnum == EquipmentCmdEnum.SET_ARM_POWER) {
            RealManArmPowerDto result = new RealManArmPowerDto();
            RealManArmPowerParamDto paramDto = (RealManArmPowerParamDto) cmdParamsDto;
            result.setArmPower(toolkit.setArmPower(paramDto.getArmPower()));
            return result;
        }
        if (cmdEnum == EquipmentCmdEnum.START_DRAG_TEACH) {
            RealManArmDragTeachDto result = new RealManArmDragTeachDto();
            RealManArmDragTeachParamDto paramDto = (RealManArmDragTeachParamDto) cmdParamsDto;
            result.setDragTeach(toolkit.setDragTech(paramDto.getTrajectoryRecord()));
            return result;
        }
        if (cmdEnum == EquipmentCmdEnum.STOP_DRAG_TEACH) {
            RealManArmDragTeachDto result = new RealManArmDragTeachDto();
            result.setDragTeach(toolkit.setStopTech());
            return result;
        }
        if (cmdEnum == EquipmentCmdEnum.GET_CURRENT_ARM_STATE) {
            return toolkit.getCurrentStatus();
        }
        if (cmdEnum == EquipmentCmdEnum.SET_INIT_POSE) {
            RealManArmSetInitPostDto result = new RealManArmSetInitPostDto();
            RealManArmAngleDto paramDto = (RealManArmAngleDto) cmdParamsDto;
            result.setInitPose(toolkit.setInitPose(paramDto.getAngle1(), paramDto.getAngle2(), paramDto.getAngle3()
                    , paramDto.getAngle4(), paramDto.getAngle5(), paramDto.getAngle6(), paramDto.getAngle7()));
            return result;
        }
        if (cmdEnum == EquipmentCmdEnum.GET_INIT_POSE) {
            RealManArmInitPostDto result = new RealManArmInitPostDto();
            result.setInitPose(toolkit.getInitPose());
            return result;
        }
        if (cmdEnum == EquipmentCmdEnum.MOVE_JOINTS) {
            RealManArmBaseDto result = new RealManArmBaseDto();
            RealManArmAngleDto paramDto = (RealManArmAngleDto) cmdParamsDto;
            result.setResult(toolkit.moveJoints(paramDto.getAngle1(), paramDto.getAngle2(), paramDto.getAngle3()
                    , paramDto.getAngle4(), paramDto.getAngle5(), paramDto.getAngle6(), paramDto.getAngle7(), paramDto.getRate()));
            return result;
        }
        if (cmdEnum == EquipmentCmdEnum.SET_ARM_STOP) {
            RealManArmBaseDto result = new RealManArmBaseDto();
            result.setResult(toolkit.setArmStop());
            return result;
        }
        if (cmdEnum == EquipmentCmdEnum.SET_ARM_PAUSE) {
            RealManArmBaseDto result = new RealManArmBaseDto();
            result.setResult(toolkit.setArmPause());
            return result;
        }
        if (cmdEnum == EquipmentCmdEnum.SET_ARM_CONTINUE) {
            RealManArmBaseDto result = new RealManArmBaseDto();
            result.setResult(toolkit.setArmContinue());
            return result;
        }
        if (cmdEnum == EquipmentCmdEnum.SET_DELETE_CURRENT_TRAJECTORY) {
            RealManArmBaseDto result = new RealManArmBaseDto();
            result.setResult(toolkit.setDeleteCurrentTrajectory());
            return result;
        }
        return null;
    }

}
